#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main()
{
    // 建立世界坐标系与小萝卜一号坐标系之间的转换矩阵
    Quaterniond q1(0.55,0.3,0.2,0.2);
    q1=q1.normalized();//四元素归一化
    Matrix<double,3,1> t1;
    t1 << 0.7,1.1,0.2; //平移向量
    Isometry3d Tcw1=Isometry3d::Identity();//创建欧式群
    Tcw1.rotate(q1.toRotationMatrix());//四元数转化为旋转矩阵
    Tcw1.pretranslate(t1);//四元数转化为平移矩阵

    // 建立世界坐标系与小萝卜二号坐标系之间的转换矩阵
    Quaterniond q2(-0.1,0.3,-0.7,0.2);
    q2=q2.normalized();
    Matrix<double,3,1> t2;
    t2<<-0.1,0.4,0.8;
    Isometry3d Tcw2=Isometry3d::Identity();//创建Tcw2的欧式群
    Tcw2.rotate(q2.toRotationMatrix());
    Tcw2.pretranslate(t2);
        
    // p1点在小萝卜一号坐标系上的坐标
    Matrix<double,3,1> p1;
    p1<<0.5,-0.1,0.2;

    // 将p1点先表示在世界坐标系中，然后转移到小萝卜一号坐标系上的坐标
    Matrix<double,3,1> p2;
    p2 = Tcw2*Tcw1.inverse()*p1;
    
    cout<<p2<<endl;
        
    return 0;
}

// Results:
// 1.08228
// 0.663509
// 0.686957
